#include <SPI.h> 
#include <SoftwareSerial.h>
#include <Streaming.h>

#define VSYNC A2// INT0
#define MAX7456SELECT 10//ss
#define MAXReset A3
//MAX7456 opcodes
#define DMM_reg   0x04
#define DMAH_reg  0x05
#define DMAL_reg  0x06
#define DMDI_reg  0x07
#define VM0_reg   0x00
#define VM1_reg   0x01

//MAX7456 commands
#define CLEAR_display 0x04
#define CLEAR_display_vert 0x06
#define END_string 0xff

// with PAL
// all VM0_reg commands need bit 6 set
#define ENABLE_display 0x48
#define ENABLE_display_vert 0x4c
#define MAX7456_reset 0x42
#define DISABLE_display 0x40

#define WHITE_level_80 0x03
#define WHITE_level_90 0x02
#define WHITE_level_100 0x01
#define WHITE_level_120 0x00

// with PAL
#define MAX_screen_size 480
#define MAX_screen_rows 16

volatile byte screen_buffer[MAX_screen_size];

volatile byte writeOK;
volatile byte valid_string;
volatile byte save_screen;
volatile int  incomingByte;
volatile int  count;

String Screenlayout[15];


#define x A0
#define y A1
#define speedsel A2
#define b1 4
#define b2 5
#define b3 6
#define b4 7
#define b5 8
#define b6 9

int commands[10];
String RecieveCommands[10];

int xval=0;
int yval=0;
int speedval=0;
int b1val=0;
int b2val=0;
int b3val=0;
int b4val=0;
int b5val=0;
int b6val=0;

SoftwareSerial xBee(2, 3); // RX, TX

void setup(){
  SPI.begin(); 
  Serial.begin(9600);
  delay(200);
  Serial.println("Loading");
  xBee.begin(115200);
  pinMode(MAX7456SELECT,OUTPUT);
  digitalWrite(MAX7456SELECT,HIGH); //disable device
  pinMode(VSYNC, INPUT);
  pinMode(MAXReset,OUTPUT);
  // SPCR = 01010000
  //interrupt disabled,SPI enabled,msb 1st,master,clk low when idle,
  //sample on leading edge of clk,system clock/4 rate (4 meg)
  //  SPCR = (1<<SPE)|(1<<MSTR);
  //  SPI_junk=SPSR;
  //  SPI_junk=SPDR;
  delay(250);

  writeOK = false;
  valid_string = false;
  save_screen = false;
  incomingByte = 0;
  count = 0;
  pinMode(MAXReset,OUTPUT);
  delay(5000);
  digitalWrite(MAXReset,HIGH);
  //  Screenlayout[8] = "Loading";
//  write_new_screen("Loading...");


}

void loop(){

  xval = analogRead(x);
  yval = analogRead(y);
  speedval = analogRead(speedsel);
  b1val = digitalRead(b1);
  b2val = digitalRead(b2);
  b3val = digitalRead(b3);
  b4val = digitalRead(b4);
  b5val = digitalRead(b5);
  b6val = digitalRead(b6);
  String OutputxBee="";
    OutputxBee = (String)xval;
    OutputxBee += ":";
    OutputxBee += (String)yval;
    OutputxBee += ":";
    OutputxBee += (String)speedval;
    OutputxBee += ":";
    OutputxBee += (String)b1val;
    OutputxBee += ":";
    OutputxBee += (String)b2val;
    OutputxBee += ":";
    OutputxBee += (String)b3val;
    OutputxBee += ":";
    OutputxBee += (String)b4val;
    OutputxBee += ":";
    OutputxBee += (String)b5val;
    OutputxBee += ":";
    OutputxBee += (String)b6val;
    OutputxBee += ":";
  
  Serial.println(OutputxBee);

  //  String output="";
  //  output += 
  if(b1val){
    
    xBee.println(OutputxBee);
  }
  
  
  
  if(b2val){

    byte incomingByte;
    String txtMsg2;
    while(Serial.available() > 0) {
      char in_char2=0;
      in_char2=Serial.read();
      incomingByte = in_char2;
//      if (incomingByte == 0x0d)
      if (in_char2 == '@')      
      {
        SplitString(txtMsg2);
        txtMsg2 = ""; 
      }
      else
      {
        txtMsg2 += in_char2;
      }
    }
    xBee << commands[0] << ":" << commands[1] << ":" << commands[2] << ":" << commands[3] << ":" << commands[4] << ":" << commands[5] << ":" << commands[6] << ":" << commands[7] << ":" << commands[8] << ":" << "@" << endl;  

  }

  byte incomingByte3;
  String txtMsg3;
  //  int loo=0;
  if(xBee.available()){
    while(xBee.available() > 0) {
      //    Serial.println(loo);
      //    loo +=1;
      char in_char3=0;
      in_char3=xBee.read();
      //    Serial.println(in_char3);
      incomingByte3 = in_char3;
//      if (incomingByte3 == 0x0d)
      if (in_char3 == '@')
      {
        int i, count; 
        for (i=0, count=0; txtMsg3[i]; i++) 
          count += (txtMsg3[i] == ':'); 

        for(int i =0; i < count;i++){
          int firstpos = txtMsg3.indexOf(":");
          RecieveCommands[i] = txtMsg3.substring(0,firstpos);
          txtMsg3 = txtMsg3.substring(firstpos+1,txtMsg3.length());
        }

        xBee.flush();
        txtMsg3 = ""; 
        continue;
      }
      else
      {
        txtMsg3 += in_char3;
      }
    }
  }      

  String DisOutput = "Distance: ";
  DisOutput += RecieveCommands[0];
  //RecieveCommands[0] = "";
  Serial.println (DisOutput);
  Screenlayout[1] = DisOutput;

  write_new_screen(DisOutput);
  xBee.flush();
delay(500);
}

String SplitString(String Data){

  int i, count; 
  for (i=0, count=0; Data[i]; i++) 
    count += (Data[i] == ':'); 

  for(int i =0; i < count;i++){
    int firstpos = Data.indexOf(":");
    commands[i] = convint(Data.substring(0,firstpos));
    Data = Data.substring(firstpos+1,Data.length());
  }
}
String SplitString2(String Data){
  // Serial.println(Data);
  int i, count; 
  for (i=0, count=0; Data[i]; i++) 
    count += (Data[i] == ':'); 

  for(int i =0; i < count;i++){
    int firstpos = Data.indexOf(":");
    RecieveCommands[i] = Data.substring(0,firstpos);
    Data = Data.substring(firstpos+1,Data.length());
  }
}

int convint(String Data){
  char carray2[Data.length() +1];
  Data.toCharArray(carray2,sizeof(carray2));
  return atoi(carray2); 
}

//////////////////////////////////////////////////////////////
byte convert_ascii (int character)
{
  // for some reason the MAX7456 does not follow ascii letter
  // placement, so you have to have this odd lookup table
  // todo... create an eeprom table that matches ascii
  // and burn to the MAX7456

  byte lookup_char;

  if (character == 32)
    lookup_char = 0x00; // blank space
  else if (character == 48)
    lookup_char = 0x0a; // 0
  else if ((character > 48) && (character < 58))
    lookup_char = (character - 48); // 1-9
  else if ((character > 64) && (character < 90))
    lookup_char = (character - 54); // A-Z
  else if ((character > 96) && (character < 123))
    lookup_char = (character - 60); // a-z
  else if (character == 34)
    lookup_char = 0x48; // "
  else if (character == 39)
    lookup_char = 0x46; // '
  else if (character == 40)
    lookup_char = 0x3f; // (
  else if (character == 41)
    lookup_char = 0x40; // )
  else if (character == 44)
    lookup_char = 0x45; // ,
  else if (character == 45)
    lookup_char = 0x49; // -
  else if (character == 46)
    lookup_char = 0x41; // .
  else if (character == 47)
    lookup_char = 0x47; // /
  else if (character == 58)
    lookup_char = 0x44; // :
  else if (character == 59)
    lookup_char = 0x43; // ;
  else if (character == 60)
    lookup_char = 0x4a; // <
  else if (character == 62)
    lookup_char = 0x4b; // >
  else if (character == 63)
    lookup_char = 0x42; // ?
  else
    lookup_char = 0x00; // out of range, blank space

  return (lookup_char);

}

//////////////////////////////////////////////////////////////
void write_new_screen(String Data)
{
  char carray2[Data.length() +1];
  Data.toCharArray(carray2,sizeof(carray2));

  for (int x = 0;x< Data.length();x ++){
    incomingByte = carray2[x];
    //    Serial.print(incomingByte);
    screen_buffer[x] = convert_ascii(incomingByte);  
  }
  //  Serial.println("");
  int x, local_count;
  byte char_address_hi, char_address_lo;
  byte screen_char;

  local_count = Data.length();

  char_address_hi = 0;
  char_address_lo = 0;
  //Serial.println("write_new_screen");  

  // clear the screen
  digitalWrite(MAX7456SELECT,LOW);
  SPI.transfer(DMM_reg);
  SPI.transfer(CLEAR_display);
  digitalWrite(MAX7456SELECT,HIGH);

  // disable display
  digitalWrite(MAX7456SELECT,LOW);
  SPI.transfer(VM0_reg);
  SPI.transfer(DISABLE_display);

  SPI.transfer(DMM_reg); //dmm
  //SPI.transfer(0x21); //16 bit trans background
  SPI.transfer(0x01); //16 bit trans w/o background

  SPI.transfer(DMAH_reg); // set start address high
  SPI.transfer(char_address_hi);

  SPI.transfer(DMAL_reg); // set start address low
  SPI.transfer(char_address_lo);

  x = 0;
  while(local_count) // write out full screen
  {
    screen_char = screen_buffer[x];
    SPI.transfer(DMDI_reg);
    SPI.transfer(screen_char);
    x++;
    local_count--;
  }

  SPI.transfer(DMDI_reg);
  SPI.transfer(END_string);

  SPI.transfer(VM0_reg); // turn on screen next vertical
  SPI.transfer(ENABLE_display_vert);
  digitalWrite(MAX7456SELECT,HIGH);
}


